﻿using UnityEngine;
using UnityEngine.AI;

namespace AI.Graph {


    public static class NavMeshAgentExtension {

        /// <summary>
        /// Get the distance from the agent to a destination..
        /// </summary>
        /// <returns>Returns true if the to point is reachable.</returns>
        public static bool DistanceToDestination (this NavMeshAgent agent, Vector3 destination, out float distance) {
            return PathDistance (GetPath (agent, destination), out distance);
        }


        /// <summary>
        /// Get the distance from one point to another.
        /// </summary>
        /// <returns>Returns true if the to point is reachable.</returns>
        public static bool DistanceToDestination (Vector3 from, Vector3 to, int areaMask, out float distance) {
            NavMeshPath path = new NavMeshPath ();
            NavMesh.CalculatePath (from, to, areaMask, path);
            return PathDistance (path, out distance);
        }


        /// <summary>
        /// Calculates a NavMeshPath.
        /// </summary>
        public static NavMeshPath GetPath (this NavMeshAgent agent, Vector3 to) {
            NavMeshPath path = new NavMeshPath ();
            agent.CalculatePath (to, path);
            return path;
        }


        /// <summary>
        /// Returns true if the agent can reach th given point.
        /// </summary>
        public static bool IsValidDestination (this NavMeshAgent agent, Vector3 point) {
            NavMeshPath path = new NavMeshPath ();
            bool valid = agent.CalculatePath (point, path);

            return valid && path.status != NavMeshPathStatus.PathPartial;
        }


        /// <summary>
        /// Returns true if all points are reachable with the agent.
        /// </summary>
        public static bool ValidDestinations (this NavMeshAgent agent, Vector3[] points) {
            foreach (Vector3 point in points) {
                if (!IsValidDestination (agent, point)) {
                    Debug.Log ("Destination not reachable [" + point + "]");
                    return false;
                }
            }

            return true;
        }


        /// <summary>
        /// Returns the nearest destination from the agent.
        /// </summary>
        public static bool NearestDistanceToDestination (this NavMeshAgent agent, Vector3[] destinations, out float distance) {
            if (NearestDestination (agent, destinations, out Vector3 nearestDestination)) {
                return DistanceToDestination (agent, nearestDestination, out distance);
            }

            distance = float.MaxValue;
            return false;
        }


        /// <summary>
        /// Returns the nearest destination from the agent.
        /// </summary>
        public static bool NearestDestination (this NavMeshAgent agent, Vector3[] destinations, out int destinationIndex) {
            return NearestDestination (agent, destinations, out Vector3 nearestDestination, out destinationIndex);
        }


        /// <summary>
        /// Returns the nearest destination from the agent.
        /// </summary>
        public static bool NearestDestination (this NavMeshAgent agent, Vector3[] destinations, out Vector3 destination) {
            return NearestDestination (agent, destinations, out destination, out int index);
        }


        /// <summary>
        /// Returns the nearest destination from the agent.
        /// </summary>
        public static bool NearestDestination (this NavMeshAgent agent, Vector3[] destinations, out Vector3 destination, out int destinationIndex) {
            destination = new Vector3 ();
            destinationIndex = -1;
            float shortestDistance = float.MaxValue;
            bool pathFound = false;
            for (int i = 0; i < destinations.Length; i++) {
                Vector3 position = destinations[i];
                if (agent.DistanceToDestination (position, out float calculatedDistance)) {
                    if (calculatedDistance < shortestDistance) {
                        pathFound = true;
                        shortestDistance = calculatedDistance;
                        destination = position;
                        destinationIndex = i;
                    }
                }
            }

            return pathFound;
        }


        static bool PathDistance (NavMeshPath path, out float distance) {
            float result = 0;

            if (path.status == NavMeshPathStatus.PathInvalid) {
                distance = 0;
                return false;
            }

            for (int i = 1; i < path.corners.Length; i++) {
                result += Vector3.Distance (path.corners[i - 1], path.corners[i]);
            }

            distance = result;
            return true;
        }

    }


}